
#include "Command_Mod.h"
#include "Com_Mod.h"
#include "blob_detect.h"


#include <stdio.h>
#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;
using namespace::boost::asio;  

//Method for loading video feed for single blob detection.
IplImage* GetThresholdedImage(IplImage* img, IplImage* result)
{
	// Convert the image into an HSV image
	IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);

	// Values 20,100,100 to 30,255,255 working perfect for yellow at around 6pm
	cvInRangeS(imgHSV, cvScalar(20, 100, 100), cvScalar(30, 255, 255), imgThreshed); // yellow
	//cvInRangeS(imgHSV, cvScalar(85, 100, 100), cvScalar(130, 255, 255), imgThreshed);// blue

	cvReleaseImage(&imgHSV);

	return imgThreshed;
}

//Method for single blob detection.
void ObjectDetect(IplImage* img, IplImage* result, IplImage* frame){
	int Mass = 0;
	int Y = 0;
	int X = 0;
	CvScalar PixelValue;
	CvScalar Centre;
	//BGR
	//Centre.val[1]=255;
	Centre.val[0]=255;
	cvNamedWindow("result");

	result = frame;

	for(int Row = 0; Row < img->height; Row++){
		for(int Col = 0; Col < img->height; Col++){
			PixelValue = cvGet2D(img, Row, Col);

			if(PixelValue.val[0] == 255){
				Mass++;
				Y = Y + Row;
				X = X + Col;
			}
		};
	};	
	if(Mass != 0 || Y != 0 || X != 0){
		Y = Y/Mass;
		X = X/Mass;
		cvSet2D(result, Y, X, Centre); 
	}
	else{
		cout << "No object detected" << endl;
	};
};

int main()
{
	int X = 0;
	int Y = 0;
	//CommandMenu menu;
	BlobDetector blob;
	blob.LastTransX = 0;
	blob.LastTransY = 0;
	VideoCapture Cam(1);
	Cam >> blob.Frame;

	Communicator Comm;
	cout << "Setting up communication module" << endl;
	Comm.Setup();
	cout << "Communication module running" << endl;

	//Needs to wait for the Arduino to give it time to setup the serial connection.
	Sleep(3000);
   

	while(true)
	{
		blob.LoadLiveFeed(Cam);
		blob.Image_Scan();
		cout << "Enter x angle: ";
		cin >> X;
		cout << endl;
		cout << "Enter y angle: "; 
		cin >> Y;
		cout << endl;

		if ((blob.LastMassCol != blob.LastTransX) || (blob.LastMassRow != blob.LastTransY))
		{
			//X++;
		    //Y++;
			Comm.Send(blob.LastMassCol, blob.LastMassRow, X, Y);

			cout << "Send: " << blob.LastMassRow << ' ' <<blob.LastMassCol << ' ' << Y << ' ' << X<< ' ' << endl;
			blob.LastTransX = blob.LastMassCol;
			blob.LastTransY = blob.LastMassRow;
		
			Comm.Recieve();

			cout << "Recv: " << Comm.RecvData.Struc.Y << ' ' << Comm.RecvData.Struc.X << ' ' << Y << ' ' << X << ' ' << endl;
			cout << endl;
		}
		cvWaitKey(1);
	};

	return 0;
}                                                                                                                                                     